\subsection{RobWorkSim}
For grasp simulation, the RobWorkSim framework is used, see \cite{robworksim}. A plugin has been developed for RobWork, which uses the RobWorkSim framework for the dynamic grasp simulation. For a brief description of installation and usage of the software, see appendix \ref{software}.\\
RobWorkSim is able to use the mentioned quality measures. In order to examine different grasp quality measures, some changes are made. First the basic components of the framework is explained, followed by a explanation of the custom made changes for the purpose of this project.

\subsubsection{Open Dynamics Engine}
Open Dynamics Engine (ODE) is a physics engine. The purpose of the physics engine is to find the velocities of different objects, when they are subject to the constraint that they must not penetrate. It is possible to put external forces on the objects, and ODE will impose contact forces in order to satisfy the motion constraints. ODE will therefore find the contact points where the objects collide, and the contact forces acting at these contact points. Based on velocities, forces and torques, ODE will calculate new linear and angular velocities after a certain discrete timestep.\\
ODE works in the velocity domain, which has the inherent problem of drift in position. To account for this, ODE uses a position penalty, that introduces a small force if the position is to far off.\\
One step of the ODESimulator can be expressed as:
\begin{verbatim}
step(dt)
  find contacts
  calculate constraints
  do
   solve dynamics
   check for penetrations
   if penetration
     dt = dt / 2
  while penetration
  update time
\end{verbatim}

\subsubsection{Contact Generation Problem}
RobWork uses triangles to represent the geometry of objects. There are however ambiguities when one need to find the contact normal of a contact between two penetrating triangles. Unfortunately the simulator will break down if the objects should penetrate. Therefore a workaround is used which introduces a small shell around each object. When the objects do not touch, it is possible to find the contact point where the objects in contact is closest to each other. It is then straightforward to find the contact normal from the two closest points. Objects can be said to be in contact when the shells penetrate, see figure \ref{shell}.

\begin{figure}[H]
\centering
\includegraphics[width =0.3 \textwidth]{figures/shell.png}
\caption{An illustration of a segment of an object and how a contact is present when the surrounding shell is penetrated by the gripper.}
\label{shell}
\end{figure}

\subsubsection{GraspTaskSimulator}
The GraspTaskSimulator of RobWorkSim is used. GraspTaskSimulator is a state machine that simulate a certain grasp procedure, as depicted in the statediagram in figure \ref{fig:state}. 
\begin{figure}[H]
\centering
\includegraphics[width = 0.5\textwidth]{figures/statediagram.png}
\caption{Statediagram of the GraspTaskSimulator.}
\label{fig:state}
\end{figure}
\noindent The state machine is divided into four states, respectively NewGrasp, Approach, Grasping and Lifting each taking care of the corresponding part and responding with a status of the simulation. The status can be success, SimulationFailure, InCollision, TimeOut, ObjectMissed, ObjectDropped and Slipped.\\


\noindent As physics engine ODE is used as explained earlier. The physics engine is wrapped in a DynamicSimulator which is wrapped again in a ThreadSimulator. The ThreadSimulator creates a separate thread for the simulator. It is possible to either make a single step at a time, or let the simulator run continuously. A callback is used to notify the GraspTaskSimulator each time a step has been performed.\\
The DynamicSimulator uses the ODE physics engine, and a PD controller for the bodies that are simulated. For kinematic bodies the angular and linear velocities can be directly calculated such that a body is in a given pose in the next time step. For dynamic bodies the wrenches must be used with a PD controller to force the body towards a specific pose in the next timestep. If, as an example, a hand should be moved from one pose to another, the hand could be regarded as either kinematic or dynamic. \\
The ODESimulator is a interface to ODE, which fits into the PhysicsEngine interface of RobWork. It is possible to add bodies, devices, sensors and controllers to the PhysicsEngine, as well as stepping through the simulation. Sensors is used to find contact points, and contact forces, where controllers are used to apply forces in order to move the objects in a certain motion. The controller is therefore the input for the simulator, and the sensor is the output of the simulator.

\subsubsection{Grasp sampling strategy}
The grasp tasks are generated at random. Each grasp has a unique pose relative to the object. A SurfacePoseSampler is used to do this sampling. It uses the surface geometry (a trimesh) of the object. A list of all triangles are generated with a cumulative area. A random number is chosen, which identifies a certain triangle on the surface. A binary search is performed to find the triangle, and a random point is chosen on the triangle. A random rotation is then found, and the position is retracted a certain distance from the object in the given orientation. This distance is set to 2 cm.

\subsubsection{Modifications to the Framework}
The classes GraspTaskSimulator, GWSMeasure3D and QHullND has been modified. The modified classes is named OwnGraspTaskSimulator, OwnGWSMeasure3D, and OwnQHullND respectively. The modifications falls in three categories: data extraction, method modification, and determinacy.\\
The modifications of the three classes is described in the following:
\begin{description}
\item[OwnGraspTaskSimulator] Has been modified to write out more data to the result (the contact points position, normal and normal force). In the lifting state the property "AccWGripperClosed" from the input is read. The mass of the object is multiplied with this value to give a force, which is applied in the centre of gravity on the object. The getObjectContacts method has also been modified, such that the found contacts are sorted according to normal force, and contact position x, y and z. This is done as the order was found to be random. In this method the contact forces is also scaled according to the scaling method mentioned earlier. The multiplication factor for the torques was altered in the method calcGraspQuality as mentioned earlier.
\item[OwnQHullND] Two methods has been added: subspace and getMinWrench. The subspace method uses qhull to compute the intersection between a 3D space and a 6D convex hull. The getMinWrench method is used to find a worst case wrench of a 6D convex hull.
\item[OwnGWSMeasure3D] The quality method has been modified such that it uses the new subspace method of OwnQHullND. First a ordinary 6D wrench space is built. The new getMinWrench method of OwnQHullND is then used to find the worst case torque that can be applied. The 3D subspace of the 6D convex hull is then found. This results in a new 3D convex hull, from which the minimum distance from the origo is found and used as the quality measure.
\end{description}

\subsubsection{Overall Algorithm}
Given the above mentioned  parts the overall simulation algorithm is condensed to the following. This is the overall algorithm implemented in the main RWSimPlugin class.
\begin{verbatim}
generate targets (surfaceposesampler)
run outer statemachine (7 states)
  for all targets
    reload dynamic workcell
    create new simulator instance
    generate input to simulator (1 target, specific acceleration)
    start simulator and wait
    save result
\end{verbatim}